package com.test;

import com.sun.media.sound.FFT;

import javax.sound.sampled.*;
import java.io.*;
import java.util.*;

/*

 play -n  synth 0.001 sin 10000
 play -n  synth 0.1 sin 1000 vol 0.1
 play -n  synth 0.2 sin 1000 vol 0.01
 play -n  synth 0.2 sin 1000 vol 0.009
 play -n  synth 0.2 sin 1500 vol 0.02


gnuplot -e "set datafile separator ','; set output 'outfile1.png'; set terminal png ; plot '1.txt' using 1:2 with lines, '' using 1:3 with lines"

 */
public class App 
{
  final double SOUND_SPEED = 340.29; // meter / sec
  static final int SAMPLE_RATE = 48000;
  static final int FREQ = 8000; //1500;
  static File dataDir = new File("/tmp/adir");

    public static void main( String[] args ) throws Exception
    {
      dataDir.mkdir();
      //new App().testBandPass();
      new App().playTicksThread();
      //new App().listen();
      //new App().testSinus();

      //new App().doppler();
      //new App().playToneThread(16000);
    }
  
  void doppler() throws Exception{
    final int BASE_FREQ = 16000;
    playToneThread(BASE_FREQ);
    final double MIN_FREQ = BASE_FREQ - BASE_FREQ/1000*2; // max speed is 1/1000 of sound speed, 30 cm/sec
    final double MAX_FREQ = BASE_FREQ + BASE_FREQ/1000*2; // max speed is 1/1000 of sound speed, 30 cm/sec

    TargetDataLine line = openMikeLine(140000);

    int bufSize=2;
    while(bufSize < line.getBufferSize() / 2){
      bufSize *= 2;
    }
    System.out.println("bufSize "+bufSize);
    byte[] data = new byte[bufSize];

    long t0 = System.currentTimeMillis();

    //double cumulativeFreqDelta=0;
    double cumulativeDistCm=0;
    for(int block=1; ; block++) {
      // Read the next chunk of data from the TargetDataLine.
      int numBytesRead =  line.read(data, 0, data.length);
      // Save this chunk of data.
      int[] bb = convertSound(data);
      double[] freq = fft(bb);
      if( block==1 ){
        System.out.println("sumAbsDiff="+sumAbsDiff(bb));
        System.out.println("freq resolution "+((double)SAMPLE_RATE/2/freq.length)+" Hz");
      }

      //MaxState maxState = new MaxState();
      CurveApproxMax curveApproxMax = new CurveApproxMax();
      int rangeStartIdx = -1;
      for( int i=0; i<freq.length; i++ ){
        double freqi = ((double)SAMPLE_RATE/2*i/freq.length);
        if( freqi > MIN_FREQ && freqi < MAX_FREQ ){
          curveApproxMax.add(freqi, freq[i]);
          if( rangeStartIdx==-1 ){
            rangeStartIdx = i;
          }
//          if( freq[i]>freq[i-1] && freq[i]>freq[i+1] ){
//            System.out.printf("%.1f Hz   %.0f  localMax\n", freqi, freq[i]);
//          }
          //maxState.add(freq[i]);
        }
      }
      double approxMax = curveApproxMax.approxMax();
      double speedSmSec = -(approxMax - BASE_FREQ)/BASE_FREQ*SOUND_SPEED*100;
      cumulativeDistCm += speedSmSec * bb.length / SAMPLE_RATE;
      //cumulativeFreqDelta += (approxMax - BASE_FREQ);
      System.out.printf("curveApproxMax %.1f speed cm/sec %.1f  comulativeDistCm %.0f \n",
          approxMax, speedSmSec, cumulativeDistCm);
      /*
      System.out.println();
      if( maxState.ok && maxState.idxMax>=0 ){
        int i = rangeStartIdx + maxState.idxMax;
        double freqi = ((double)SAMPLE_RATE/2*i/freq.length);
        System.out.println( freqi + "Hz max ");

      }
      */

    }    
  }

  /*
  class MaxState{
    double prevValue;
    int idx=0;
    int idxMax=-1;
    boolean ok=true;
    void add(double value){
      if( idx>0 ){
        if( idxMax==-1 ){
          if( value < prevValue ){
            idxMax=idx;
          }
        }else if( value > prevValue ){
          ok = false;
        }
      }
      prevValue=value;
      idx ++;
    }
  }
  */
  
  class CurveApproxMax{ // center of mass calculation
    NavigableMap<Double, Double> data = new TreeMap<>();
    double maxPoint;
    void add(double point, double value){
      if( data.size()==0 ){
        maxPoint = point;
      }else if( value > data.get(maxPoint) ){
        maxPoint = point;
      }
      data.put(point, value);
    }
    double approxMax(){
      double range = Math.min( maxPoint - data.firstKey(), data.lastKey() - maxPoint );
      double totalW=0;
      double totalMoment=0;
      for( Map.Entry<Double,Double> me : data.entrySet() ){
        if( Math.abs(me.getKey()-maxPoint)<=range ){
          totalW += me.getValue();
          totalMoment += me.getValue()*me.getKey();
        }
      }
      return totalMoment / totalW;
    }
  }
  
  double[] fft(int[] data){
    FFT fft = new com.sun.media.sound.FFT(data.length, 1);
    double[] dd = new double[data.length*2];
    for( int i=0; i<data.length; i++ ){
      dd[i*2]=data[i];
    }
    fft.transform(dd);

    // for pure real source signal FFT symmetrical - but complex conjugate
    // dd[992]=d[32]   d[993]=d[33] - d.length=1024

    //verify symmetrical - except for first 2 elements
    for( int i=1; i<data.length; i++ ){
      if( Math.abs( dd[i*2] - dd[dd.length-i*2] ) > 0.0001 ){
        throw new IllegalStateException();
      }
      if( Math.abs( dd[i*2+1] + dd[dd.length-i*2+1] ) > 0.0001 ){ // complex conjugate
        throw new IllegalStateException();
      }
    }

    double[] ret = new double[data.length/2];
    for( int i=0; i<ret.length; i++ ){
      ret[i] = Math.sqrt(dd[i*2]*dd[i*2] + dd[i*2+1]*dd[i*2+1]);
    }
    return ret;
  }
  
  void playTicksThread(){
    new Thread(){
      @Override
      public void run() {
        try {
          playTicks();
        } catch (Exception e) {
          throw new RuntimeException(e);
        }
      }
    }.start();
  }

  void playToneThread(final int freq){
    new Thread(){
      @Override
      public void run() {
        try {
          playTone(freq);
        } catch (Exception e) {
          throw new RuntimeException(e);
        }
      }
    }.start();
  }


  void playTone(int freq) throws Exception{
    byte[] buf = new byte[SAMPLE_RATE*10];

    for (int i=0; i< buf.length; i++) {
      double angle = (double)i / (SAMPLE_RATE / freq) * 2.0 * Math.PI;
      byte val = (byte)(Math.sin(angle) * 127.0 / 5);
      buf[i] = val;
    }
    AudioFormat af = new AudioFormat(SAMPLE_RATE,8,1,true,false);
    SourceDataLine sdl = AudioSystem.getSourceDataLine(af);
    sdl.open(af);
    sdl.start();
    for(;;){
      sdl.write(buf,0,buf.length);
      //sdl.drain();
    }
    //sdl.close();
  }

  byte[] dup(byte[] a, int count){
    byte[] ret = new byte[a.length*count];
    for( int i=0; i<ret.length; i++ ){
      ret[i] = a[i%a.length];
    }
    return ret;
  }

  void playTicks() throws Exception{

    
    byte[] buf = new byte[SAMPLE_RATE];

    int period = SAMPLE_RATE / FREQ;
    for (int i=0; i< period * 16; i++) {
      if( (i/period) % 3 == 1 ){
        double angle = (double)i / (SAMPLE_RATE / FREQ) * 2.0 * Math.PI;
        byte val = (byte)(Math.sin(angle) * 127.0 );
        buf[i] = val;
      }
    }

    AudioFormat af = new AudioFormat(SAMPLE_RATE,8,1,true,false);

    /*
    AudioSystem.write(new AudioInputStream(new ByteArrayInputStream(buf),af,buf.length),
        AudioFileFormat.Type.WAVE, new File("/tmp/java_sine.wav"));

    if( true ){
      return;
    }
    */

/*
    Mixer.Info[] infos = AudioSystem.getMixerInfo();
    Mixer.Info myMixer = null;
    for( Mixer.Info mi : infos ){
      System.out.println("sound devices "+mi.getDescription());
      if( mi.getDescription().equals("Direct Audio Device: USB Sound Device, USB Audio, USB Audio") ){
        myMixer = mi;
      }

      try{
        SourceDataLine sdl = AudioSystem.getSourceDataLine(af, mi);
        sdl.open(af);
        sdl.start();
        sdl.write(buf,0,buf.length);
        sdl.drain();
        sdl.close();
      }catch (Exception e){
        e.printStackTrace();
      }
    }

*/
    SourceDataLine sdl = AudioSystem.getSourceDataLine(af);
    byte[] bufDup = dup(buf, 16);
    sdl.open(af, buf.length*32);
    sdl.start();
    for(;;){
      sdl.write(bufDup,0,bufDup.length);
      //sdl.drain();
    }
    //sdl.close();
  }

  void testBandPass() throws Exception{
    List<Integer> orig = new ArrayList();
    BufferedReader br = new BufferedReader(new FileReader("/home/denny/proj/scala-agent3/2013/dopler/for_bandpass_filter1.txt"));
    String line;
    while((line=br.readLine())!=null){
      String[] fields = line.trim().split(",");
      orig.add(Integer.parseInt(fields[1]));
    }
    System.out.println("orig count "+orig.size());
  }

  void testSinus() throws Exception{
    IntSource intSrc = new IntSource();
    int freq=1500;
    GarmonicFinder gf1000 = new GarmonicFinder(/*freq*/32);

    int len = SAMPLE_RATE / freq * 16;
    int[] wave = new int[len];
    for( int i = 0; i<len; i++ ){
      wave[i] = (int)(100*Math.sin(2*Math.PI*freq/SAMPLE_RATE*i));
     // wave[i] = (int)(Math.random()*1000);
    }
    
    highPass(wave);

    intSrc.add(wave);
    //System.out.println("check "+ gf1000.check(intSrc,0));
    System.out.println("check "+ gf1000.avgDeltaRatio(intSrc, 0, 16, null));
  }
  
  static int[] highPass(int[] data){
    // Sign = -1 is FFT, 1 is IFFT (inverse FFT)
    // Data = Interlaced double array to be transformed.
    // The order is: real (sin), complex (cos)
    // Framesize must be power of 2
    FFT fft = new com.sun.media.sound.FFT(data.length, 1);
    FFT ifft = new com.sun.media.sound.FFT(data.length, -1);
    double[] dd = new double[data.length*2];
    for( int i=0; i<data.length; i++ ){
      dd[i*2]=data[i];
    }
    fft.transform(dd);

    // for pure real source signal FFT symmetrical - but complex conjugate
    // dd[992]=d[32]   d[993]=d[33] - d.length=1024
    
    //verify symmetrical - except for first 2 elements
    for( int i=1; i<data.length; i++ ){
      if( Math.abs( dd[i*2] - dd[dd.length-i*2] ) > 0.00001 ){
        throw new IllegalStateException();
      }
      if( Math.abs( dd[i*2+1] + dd[dd.length-i*2+1] ) > 0.00001 ){ // complex conjugate
        throw new IllegalStateException();
      }
    }

    dd[0]=0;
    dd[1]=0;
    ifft.transform(dd);

    int[] ret = new int[data.length];
    for( int i=0; i<data.length; i++ ){
      ret[i] = (int)(dd[i*2]/data.length/2);
    }
    return ret;
  }

  void listen() throws Exception{
    TargetDataLine line = openMikeLine(16384);

     // ByteArrayOutputStream out  = new ByteArrayOutputStream();
      int numBytesRead;
      
      int bufSize=2;
      while(bufSize < line.getBufferSize() / 5){
        bufSize *= 2;
      }
      System.out.println("bufSize "+bufSize);
      //byte[] data = new byte[line.getBufferSize() / 5 / 2 * 2];
      byte[] data = new byte[bufSize];

// Begin audio capture.
      line.start();

      long t0 = System.currentTimeMillis();
      IntSource intSrc = new IntSource();
//      GarmonicFinder gf1000 = new GarmonicFinder(1000);
//      GarmonicFinder gf500 = new GarmonicFinder(500);
      GarmonicFinder gfMainFreq = new GarmonicFinder(6);
      GarmonicFinder gfOtherFreq = new GarmonicFinder(12);

      long posToTest=0;
      final int MULTI_SIZE = 16;
      int testPieceSize = MULTI_SIZE * Math.max(gfMainFreq.len, gfOtherFreq.len);

      long lastSignalPos = - 10 * SAMPLE_RATE;
      long firstSignalPos = -1;
      int signalNo=0;
      for(int block=1; ; block++) {
        numBytesRead =  line.read(data, 0, data.length);
        int[] bb = convertSound(data);
        if( block==1 ){
          System.out.println("sumAbsDiff="+sumAbsDiff(bb));
        }
        if(sumAbsDiff(bb)==0){
          throw new Exception("no sound input");
        }
        intSrc.add(bb);
        int countSeen=0;
        for ( ; posToTest + testPieceSize - 1 <= intSrc.maxPos(); posToTest++ ){
          int check1000 = gfMainFreq.check(intSrc,intSrc.startPos+intSrc.a.length);
          double ratio1000 = gfMainFreq.avgDeltaRatio(intSrc, posToTest, MULTI_SIZE, null);
          double ratio500 = gfOtherFreq.avgDeltaRatio(intSrc, posToTest, MULTI_SIZE, null);
          int check500 = gfOtherFreq.check(intSrc,intSrc.startPos+intSrc.a.length);
          if( /*check1000>200*/ ratio1000 > 0.5 /*&& ratio500<0.1*/ ){
            countSeen++;
          }else{
            countSeen=0; // like button de-bounce
          }
          if( countSeen>=10 ){
            if( lastSignalPos < posToTest - SAMPLE_RATE / 2 ){
              if( firstSignalPos == -1 ){
                firstSignalPos = posToTest;
              }else{
                signalNo++;
              }
              /*
              for( int i=0; i<15; i++ ){
                System.out.println(intSrc.at(posToTest+i));
              }
              */
              long totalDeltaPos = posToTest-firstSignalPos-signalNo*SAMPLE_RATE;
              double totalDeltaCm = (double)totalDeltaPos/SAMPLE_RATE*SOUND_SPEED*100;
              System.out.printf("%5d %.0f cm  %5d %5d %5d  %.5f %.5f \n",
                  totalDeltaPos /*posToTest - lastSignalPos*/,
                  totalDeltaCm,
                  check1000, check500,
                  sumAbsDiff(bb),ratio1000, ratio500);
              lastSignalPos = posToTest;
            }
            /*
            File ftxt = new File(dataDir,block+".txt");
            File fpng = new File(dataDir,block+".png");
            try(Writer w = new FileWriter(ftxt)){
              gfMainFreq.avgDeltaRatio(intSrc, intSrc.startPos + intSrc.a.length, 16, w);
            }
            runOsCommand("gnuplot", "-e",
                "set datafile separator ','; set output '"+fpng.getPath()
                    +"'; set terminal png ; plot '"+ftxt.getPath()+"' using 1:2 with lines, '' using 1:3 with lines, '' using 1:4 with lines");
                    */
          }
        }
       // out.write(data, 0, numBytesRead);
      }


      //System.out.println( "Hello World!" );
    }

  private TargetDataLine openMikeLine(int lineBuf) throws LineUnavailableException {
    AudioFormat format = new AudioFormat(SAMPLE_RATE,16,1,true,true);
    TargetDataLine line;
    DataLine.Info info = new DataLine.Info(TargetDataLine.class,
        format); // format is an AudioFormat object
    if (!AudioSystem.isLineSupported(info)) {
      // Handle the error ...
    }
    // Obtain and open the line.
    line = (TargetDataLine) AudioSystem.getLine(info);
    line.open(format, lineBuf);
    System.out.println(line.getFormat());
    line.start();
    return line;
  }

  static int[] convertSound(byte[] signedBigEndian){
      int[] ret = new int[signedBigEndian.length/2];
      for( int i=0; i<ret.length; i++ ){
        ret[i] = signedBigEndian[i*2] << 8 | (signedBigEndian[i*2+1] & 0xff);
      }
      return ret;
    }
  
    static long sumAbsDiff(int[] vals){
      long v=0;
      for( int i=1; i<vals.length; i++ ){
        v += Math.abs(vals[i-1]-vals[i]);
      }
      return v;
    }


  public static List<String> runOsCommand(String... cmd) throws Exception {
    Process p = Runtime.getRuntime().exec(cmd);
    p.waitFor();
    BufferedReader reader = new BufferedReader(new InputStreamReader(p.getInputStream()));
    String line;
    List<String> ret = new ArrayList<String>();
    while ((line = reader.readLine()) != null) {
      ret.add(line);
    }
    reader.close();
    reader = new BufferedReader(new InputStreamReader(p.getErrorStream()));
    StringBuilder sbErr = new StringBuilder();
    while ((line = reader.readLine()) != null) {
      sbErr.append(line + "\n");
    }
    reader.close();
    if (sbErr.length() > 0) {
      throw new IOException(Arrays.asList(cmd) + " stderr: " + sbErr);
    }
    return ret;
  }

  
  static class IntSource{
    int[] a = new int[0];
    int[] b = new int[0];
    long startPos=0;
    void add(int[] data){
      startPos += a.length;
      a = b;
      b = data;
    }
    long maxPos(){
      return startPos + a.length + b.length - 1;
    }
    int at(long pos){
      if( pos<startPos ){
        throw new IllegalArgumentException();
      }
      if( pos >= startPos + a.length + b.length ){
        throw new IllegalArgumentException();
      }
      if( pos - startPos < a.length ){
        return a[(int)(pos-startPos)];
      }
      return b[(int)(pos-startPos-a.length)];
    }
  }
  
    static class GarmonicFinder{
      double[] sin;
      double[] cos;
      int len;
      GarmonicFinder(int len){
        //len = SAMPLE_RATE / freq;
        this.len=len;
        double freq = SAMPLE_RATE / len;
        System.out.println("freq "+freq);
        sin = new double[len];
        cos = new double[len];
        for( int i = 0; i<len; i++ ){
          sin[i] = Math.sin(2*Math.PI*freq/SAMPLE_RATE*i);
          cos[i] = Math.cos(2*Math.PI*freq/SAMPLE_RATE*i);
        }
      }
      int check(IntSource s, long at){
        double sumSin=0;
        double sumCos=0;
        for( int i=0; i<len; i++){
          //System.out.printf("%d  %.5f \n", s.at(at+i), sin[i]);
          sumSin += s.at(at+i)*sin[i];
          sumCos += s.at(at+i)*cos[i];
        }
        return (int)(Math.sqrt( sumSin*sumSin + sumCos*sumCos )*2 / len);
      }

      double avgDeltaRatio(IntSource s, long at, int multiSize, Writer gnuPlotDataOut){
        /*
        int[] filtered = null;
        if( gnuPlotDataOut!=null ){
          filtered = new int[multiSize*len];
          for( int j=0; j<multiSize; j++ ){
            for( int i=0; i<len; i++){
              int v = s.at(at + i + j*len);
              filtered[i + j*len] = v;
            }
          }
          filtered = highPass(filtered);
        }
        */


        double sumAbsDelta=0;
        double sumAbs=0;

        for( int j=0; j<multiSize; j++ ){
          double sumSin=0;
          double sumCos=0;
          for( int i=0; i<len; i++){
            //System.out.printf("%d  %.5f \n", s.at(at+i), sin[i]);
            int v = s.at(at + i + j*len);
            sumSin += v *sin[i];
            sumCos += v *cos[i];
            sumAbs += Math.abs(v);
          }

          for( int i=0; i<len; i++){
            double comp = (sin[i]*sumSin+cos[i]*sumCos)*2/len;
            //System.out.printf("%d  %.5f \n", s.at(at+i), comp);
            int v = s.at(at + i + j*len);
            sumAbsDelta += Math.abs(v - comp);

//            if( gnuPlotDataOut!=null ){
//              try {
//                gnuPlotDataOut.write((i + j*len)+","+v+","+comp+","+0/*filtered[i + j*len]*/+"\n");
//              } catch (IOException e) {
//                throw new RuntimeException(e);
//              }
//            }

          }
        }
        if( sumAbs==0 ){
          return 0;
        }
        return 1 - sumAbsDelta / sumAbs;
      }
    }
}
